#ifndef HB_AC_PID_H
#define HB_AC_PID_H

#include <stdlib.h>
#include <cmath>

#define AC_PID_FILT_HZ_DEFAULT  20.0f   // default input filter frequency
#define AC_PID_FILT_HZ_MIN      0.01f   // minimum input filter frequency


 struct ACPidInfo {
        float desired;
        float P;
        float I;
        float D;
        float FF;
        float AFF;
};


class ACPid {
public:
	// Constructor for PID
    ACPid(float initialP, float initialI, float initialD, float initialImax, float initialFiltHz, float dt);

    // set_dt - set time step in seconds
    void        setDt(float dt);

    // set_input_filter_all - set input to PID controller
    //  input is filtered before the PID controllers are run
    //  this should be called before any other calls to get_p, get_i or get_d
    void        setInputFilterAll(float input);

    // set_input_filter_d - set input to PID controller
    //  only input to the D portion of the controller is filtered
    //  this should be called before any other calls to get_p, get_i or get_d
    void        setInputFilterD(float input);

    // get_pid - get results from pid controller
    float       getPid();
    float       getPi();
    float       getP();
    float       getI();
    float       getD();
    float       getFf(float requested_rate);


	
    // reset_I - reset the integrator
    void        resetI();

    /// operator function call for easy initialisation
    void operator() (float p, float i, float d, float imaxval, float inputFiltHz, float dt );

	// get accessors
    float   &kP() { return m_kp; }
    float   &kI() { return m_ki; }
    float   &kD() { return m_kd; }
    float   &filtHz() { return m_filtHz; }
    float       getFiltAlpha() const;

    void        filtHz(const float v);

    float       getIntegrator() const { return m_integrator; }
    void        setIntegrator(float i) { m_integrator = i; }

    // set the designed rate (for logging purposes)
    void        setDesiredRate(float desired) { m_pidInfo.desired = desired; }

    const       ACPidInfo& getPidInfo(void) const { return m_pidInfo; }


	protected:

    // parameters
    float        m_kp;
    float        m_ki;
    float        m_kd;
   	float        m_imax;
    float        m_filtHz;                   // PID Input filter frequency in Hz
    float        m_ff;

    // flags
    struct ac_pid_flags {
        bool        m_resetFilter : 1;    // true when input filter should be reset during next call to set_input
    } m_flags;

    // internal variables
    float           m_dt;                    // timestep in seconds
    float           m_integrator;            // integrator value
    float           m_input;                 // last input for derivative
    float           m_derivative;            // last derivative for low-pass filter

    ACPidInfo        m_pidInfo;
	


};



#endif //HB_AC_PID_H